#include <TLorentzVector.h>

#include "CpSimpleTransporter.h"
#include <gsl/gsl_odeiv.h>
#include "CpFunctions.h"
#include "CpParticle.h"


ClassImp(CpSimpleTransporter)

CpSimpleTransporter::CpSimpleTransporter(const char* name, const char* title): CpVTransporter(name, title)
{
}


Int_t CpSimpleTransporter::Transport(Double_t t0,Double_t t1, CpTrack *track)
{

  CpPoint *point = track->GetLastPoint();
  
	const gsl_odeiv_step_type * T = gsl_odeiv_step_rk8pd;
	gsl_odeiv_step *s = gsl_odeiv_step_alloc(T, 6);
	gsl_odeiv_control *c = gsl_odeiv_control_y_new(1e-6, 0.0);
	gsl_odeiv_evolve *e = gsl_odeiv_evolve_alloc(6);

	double mu [] = { } ; //??? mu je nase mc?
	
	gsl_odeiv_system sys = {CpFunctions::GslFunc, CpFunctions::GslJac, 6, &mu};

// 	double t = 0.0, t1 = 100.0;
	double h = 1e-6;
	// x,y,z, rychlost vx, rychlost vy, rychlost vz,
  double Ip[6] = { point->GetX(), point->GetY(), point->GetZ(), point->GetVx(), point->GetVy(), point->GetVz() };
	
//    for (int i = 1; i <= t1; i++) {
//       double ti = i * t1 / t1;
		while (t0 < t1) {
			gsl_odeiv_evolve_apply(e, c, s, &sys, &t0, t1, &h, Ip);
		}

		
// 		printf("%.5e %.5e %.5e %.5e %.5e %.5e %.5e\n", t0, Ip[0], Ip[1], Ip[2], Ip[3], Ip[4], Ip[5]);
// 	}

  track->AddPoint(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5],t1);

// 	outPos->SetX(Ip[0]);
// 	outPos->SetY(Ip[1]);
// 	outPos->SetZ(Ip[2]);

// 	outVel->SetX(Ip[3]);
// 	outVel->SetY(Ip[4]);
// 	outVel->SetZ(Ip[5]);
// 	return 1;
	
	return 0;
}

